

disp(exist('ikServerStarted'))

%-------- startup --------

format long e
addpath_control
addpath([getenv('DRC_BASE'), '/software/director/src/matlab'])
robotURDF = [getenv('DRC_BASE'), '/software/models/val_description/urdf/valkyrie_A_sim_drake.urdf'];
fixed_point_file = [getenv('DRC_BASE'), '/software/control/matlab/data/val_description/valkyrie_fp_june2015.mat'];
left_foot_link = 'LeftFoot';
right_foot_link = 'RightFoot';
runIKServer

%------ startup end ------

disp(exist('ikServerStarted'))
dp.options.quat_tol = 0.0;
dp.options.tol = 0.0;
dp.options.seed_with_current = 0;

% ------ driving planner startup ------

addpath([getenv('DRC_BASE'), '/software/control/matlab/planners/driving_planner']);
clear driving_planner_options;
driving_planner_options.listen_to_lcm_flag = 0;
driving_planner_options.qstar = q_nom;
dp = drivingPlanner(s.robot, driving_planner_options);

% ------ driving planner startup end ------

environment_urdf_string = ['<?xml version="1.0"?>'...
'<robot name="affordance_environment">'...
'  <link name="link_502f301a-554f-11e5-bde2-54ee75451821">'...
'    <visual>'...
'      <origin xyz="1.25 0.1 0.98" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.07 0.07 0.25"/>'...
'      </geometry>'...
'      <material name="material_502f301a-554f-11e5-bde2-54ee75451821">'...
'        <color rgba="0.0 0.9 0.0 1"/>'...
'      </material>'...
'    </visual>'...
'    <collision>'...
'      <origin xyz="1.25 0.1 0.98" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.07 0.07 0.25"/>'...
'      </geometry>'...
'    </collision>'...
'  </link>'...
'  <link name="link_502d5cc2-554f-11e5-bde2-54ee75451821">'...
'    <visual>'...
'      <origin xyz="1.05 0.3 0.98" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.08 0.08 0.24"/>'...
'      </geometry>'...
'      <material name="material_502d5cc2-554f-11e5-bde2-54ee75451821">'...
'        <color rgba="0.9 0.9 0.1 1"/>'...
'      </material>'...
'    </visual>'...
'    <collision>'...
'      <origin xyz="1.05 0.3 0.98" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.08 0.08 0.24"/>'...
'      </geometry>'...
'    </collision>'...
'  </link>'...
'  <link name="link_502815fa-554f-11e5-bde2-54ee75451821">'...
'    <visual>'...
'      <origin xyz="1.2 0.0 0.8" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.5 1 0.06"/>'...
'      </geometry>'...
'      <material name="material_502815fa-554f-11e5-bde2-54ee75451821">'...
'        <color rgba="0.66 0.66 0.66 1"/>'...
'      </material>'...
'    </visual>'...
'    <collision>'...
'      <origin xyz="1.2 0.0 0.8" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.5 1 0.06"/>'...
'      </geometry>'...
'    </collision>'...
'  </link>'...
'  <link name="link_502a1c88-554f-11e5-bde2-54ee75451821">'...
'    <visual>'...
'      <origin xyz="1.2 0.5 0.4" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.5 0.05 0.8"/>'...
'      </geometry>'...
'      <material name="material_502a1c88-554f-11e5-bde2-54ee75451821">'...
'        <color rgba="0.66 0.66 0.66 1"/>'...
'      </material>'...
'    </visual>'...
'    <collision>'...
'      <origin xyz="1.2 0.5 0.4" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.5 0.05 0.8"/>'...
'      </geometry>'...
'    </collision>'...
'  </link>'...
'  <link name="link_502b8f5a-554f-11e5-bde2-54ee75451821">'...
'    <visual>'...
'      <origin xyz="1.2 -0.5 0.4" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.5 0.05 0.8"/>'...
'      </geometry>'...
'      <material name="material_502b8f5a-554f-11e5-bde2-54ee75451821">'...
'        <color rgba="0.66 0.66 0.66 1"/>'...
'      </material>'...
'    </visual>'...
'    <collision>'...
'      <origin xyz="1.2 -0.5 0.4" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <box size="0.5 0.05 0.8"/>'...
'      </geometry>'...
'    </collision>'...
'  </link>'...
'  <link name="link_5030ec70-554f-11e5-bde2-54ee75451821">'...
'    <visual>'...
'      <origin xyz="1.25 -0.1 0.95" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <cylinder radius="0.035" length="0.22"/>'...
'      </geometry>'...
'      <material name="material_5030ec70-554f-11e5-bde2-54ee75451821">'...
'        <color rgba="0.0 0.9 0.0 1"/>'...
'      </material>'...
'    </visual>'...
'    <collision>'...
'      <origin xyz="1.25 -0.1 0.95" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <cylinder radius="0.035" length="0.22"/>'...
'      </geometry>'...
'    </collision>'...
'  </link>'...
'  <link name="link_5032fd9e-554f-11e5-bde2-54ee75451821">'...
'    <visual>'...
'      <origin xyz="1.05 -0.2 0.95" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <cylinder radius="0.045" length="0.22"/>'...
'      </geometry>'...
'      <material name="material_5032fd9e-554f-11e5-bde2-54ee75451821">'...
'        <color rgba="0.9 0.1 0.1 1"/>'...
'      </material>'...
'    </visual>'...
'    <collision>'...
'      <origin xyz="1.05 -0.2 0.95" rpy="0.0 0.0 0.0"/>'...
'      <geometry>'...
'        <cylinder radius="0.045" length="0.22"/>'...
'      </geometry>'...
'    </collision>'...
'  </link>'...
'  <link name="link_5036ee72-554f-11e5-bde2-54ee75451821">'...
'    <visual>'...
'      <origin xyz="0.62 -1.33 0.8" rpy="0.0 0.0 -0.530773497203"/>'...
'      <geometry>'...
'        <box size="0.02 0.02 0.02"/>'...
'      </geometry>'...
'      <material name="material_5036ee72-554f-11e5-bde2-54ee75451821">'...
'        <color rgba="1 0 0 1"/>'...
'      </material>'...
'    </visual>'...
'    <collision>'...
'      <origin xyz="0.62 -1.33 0.8" rpy="0.0 0.0 -0.530773497203"/>'...
'      <geometry>'...
'        <box size="0.02 0.02 0.02"/>'...
'      </geometry>'...
'    </collision>'...
'  </link>'...
'</robot>'];
s = s.setEnvironment(environment_urdf_string);
r = s.robot_and_environment;

%-------- runMultiRRT --------

end_effector_name = 'LeftPalm';
end_effector_name_left = 'LeftPalm';
end_effector_name_right = 'RightPalm';
end_effector_pt = [];
options = struct();
options.MajorIterationsLimit = 500;
options.MajorFeasibilityTolerance = 1e-06;
options.MajorOptimalityTolerance = 0.0001;
options.FixInitialState = true;
s = s.setupOptions(options);
options.end_effector_name = end_effector_name;
options.end_effector_name_left = end_effector_name_left;
options.end_effector_name_right = end_effector_name_right;
options.end_effector_pt = end_effector_pt;
options.left_foot_link = left_foot_link;
options.right_foot_link = right_foot_link;
options.fixed_point_file = fixed_point_file;
options.graspingHand = 'left';
options.reachingElbowLink = 'LeftElbowPitchLink';
options.pelvisLink = 'Pelvis';
options.point_in_link_frame = reshape([0.07000000000039186; 0.07999999999965723; -3.427258477017858e-13], 3, []);
planner = optimalCollisionFreePlanner(r, s, [0.62, 0.0, 1.0249999999999999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.30019663134302466, 1.25, 0.0, 0.78539816339744828, 1.571, 0.0, 0.0, 0.30019663134302466, -1.25, 0.0, -0.78539816339744828, 1.571, 0.0, 0.0, 0.0, 0.0, -0.48999999999999999, 1.2050000000000001, -0.70999999999999996, 0.0, 0.0, 0.0, -0.48999999999999999, 1.2050000000000001, -0.70999999999999996, 0.0], [1.05, 0.29999999999999999, 0.97999999999999998, 1.0, 0.0, 0.0, 0.0], options);

[xGoalFull,info] = planner.findFinalPose();

[xtraj, info] = planner.findCollisionFreeTraj(xGoalFull);
if (info > 10), fprintf('The solver returned with info %d:\n',info); snoptInfo(info); end
if ~isempty(xtraj), qtraj = xtraj(1:r.getNumPositions()); else, qtraj = []; end;
if ~isempty(qtraj), qtraj_orig = qtraj; end;
if ~isempty(qtraj), joint_v_max = repmat(30.0*pi/180, r.getNumVelocities()-6, 1); end;
if ~isempty(qtraj), xyz_v_max = repmat(0.05, 3, 1); end;
if ~isempty(qtraj), rpy_v_max = repmat(2*pi/180, 3, 1); end;
if ~isempty(qtraj), v_max = [xyz_v_max; rpy_v_max; joint_v_max]; end;
if ~isempty(qtraj), v_max(r.findPositionIndices('back')) = 10.0*pi/180; end;
max_body_translation_speed = 0.5;
max_body_rotation_speed = 10;
rescale_body_ids = [];
rescale_body_pts = reshape([], 3, []);
body_rescale_options = struct('body_id',rescale_body_ids,'pts',rescale_body_pts,'max_v',max_body_translation_speed,'max_theta',max_body_rotation_speed,'robot',r);
if ~isempty(qtraj_orig), qtraj = rescalePlanTiming(qtraj_orig, v_max, 2, 0.3, body_rescale_options); end;
if ~isempty(qtraj_orig), s.publishTraj(qtraj, info); end;

%--- runMultiRRT end --------

disp(info)
